#ifndef PCL_WORLD_MODEL_IMPL_HPP_
#define PCL_WORLD_MODEL_IMPL_HPP_

//#include <pcl/world_model.h>
#include "world_model.h"


//TODO Pbl with this implementation: Points from the old world that do not have corresponding new points will NOT be erased (this happens when we revisit a place that physically changed)
//TODO base is on an octree. Simply replace parts of the world, without any change detection
//TODO use a faster filter than the conditional filter or cropbox filter



template <typename PointT>
void 
pcl::WorldModel<PointT>::setSliceAsNan(double originX, double originY, double originZ, double maxX, double maxY, double maxZ)
{
  /*
  //select points into slice
  PointCloudPtr box (new pcl::PointCloud<PointT>);

  // SET CONDITIONAL FILTER
  ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ());
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, originX)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, originX + sizeX)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, originY)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, originY + sizeY)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, originZ)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, originZ + sizeZ)));

  pcl::PointXYZI ptmin, ptmax;
  pcl::getMinMax3D	(	*world, ptmin, ptmax);

  std::cout << "world's bounds: [" << ptmin.x << "-" << ptmax.x << "] ["  << ptmin.y << "-" << ptmax.y << "] [" << ptmin.z << "-" << ptmax.z << "]" << std::endl;
  std::cout << "world contains: " << world->points.size() << " points" << std::endl;
  //pcl::io::savePCDFileASCII ("test_world.pcd", *world);

  // build the filter
  pcl::ConditionalRemoval<PointT> condrem (range_cond, true);
  condrem.setInputCloud (world);
  condrem.setKeepOrganized(false);
  // apply filter
  condrem.filter (*box);
  
  

  
  //~ IndicesConstPtr nanPoints = condrem.getIndices ();
  //set them as nans
  IndicesConstPtr nanPoints = condrem.getRemovedIndices ();
  std::cout << "======Calling set Slice as nan. Filter limits: [" << originX << " - " << originX + sizeX << "] [" << originY << " - " << originY + sizeY << "] [" << originZ << " - " << originZ + sizeZ << "]" << std::endl;
  
    std::cout << "filtered box contains " << box->points.size() << " points" << std::endl;
  
  std::cout << nanPoints->size() << " points will be set to NAN" << std::endl;
  
  std::vector<sensor_msgs::PointField> fields; 
  pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
  float my_nan = std::numeric_limits<float>::quiet_NaN ();
  for (int rii = 0; rii < static_cast<int> (nanPoints->size ()); ++rii)  // rii = removed indices iterator
  {
    std::cout << rii << " " << my_nan << std::endl;
    uint8_t* pt_data = reinterpret_cast<uint8_t*> (&world->points[(*nanPoints)[rii]]);
    for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi)  // fi = field iterator
      memcpy (pt_data + fields[fi].offset, &my_nan, sizeof (float));
  }
  
  std::cout << "SUPER TEST" << pcl_isfinite(my_nan) << std::endl;
  */
  //done
  PointCloudPtr boxAND (new pcl::PointCloud<PointT>);

  ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, originX)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, maxX)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, originY)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, maxY)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, originZ)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, maxZ)));

 
  // build the filter
  pcl::ConditionalRemoval<PointT> condremAND (range_condAND, true);
  condremAND.setInputCloud (world);
  condremAND.setKeepOrganized(false);
  // apply filter
  condremAND.filter (*boxAND);



  pcl::PointXYZI ptmin, ptmax;
  pcl::getMinMax3D	(	*world, ptmin, ptmax);
  std::cout << std::endl << "World's bounds: [" << ptmin.x << "-" << ptmax.x << "] ["  << ptmin.y << "-" << ptmax.y << "] [" << ptmin.z << "-" << ptmax.z << "]" << std::endl;
  std::cout << "Filter limits: [" << originX << " - " << maxX << "] [" << originY << " - " << maxY << "] [" << originZ << " - " << maxZ << "]" << std::endl;
  std::cout << "world contains BEFORE : " << world->points.size() << " points" << std::endl;
   //select points into slice
  PointCloudPtr box (new pcl::PointCloud<PointT>);

  // SET CONDITIONAL FILTER
  ConditionOrPtr range_cond (new pcl::ConditionOr<PointT> ());
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, originX)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, maxX)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, originY)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, maxY)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, originZ)));
  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, maxZ)));
  
  // build the filter
  pcl::ConditionalRemoval<PointT> condrem (range_cond, true);
  condrem.setInputCloud (world);
  condrem.setKeepOrganized(false);
  // apply filter
  condrem.filter (*box);
  
  
  std::cout << "points in the zone " << boxAND->points.size() << ". Points OUT of the zone " << box->points.size() << ". Points in the WORLD " << world->points.size() << std::endl;
  
  *world = *box;
  
  std::cout << "world contains AFTER filtering : " << world->points.size() << " points" << std::endl << std::endl;
  
}

template <typename PointT>
bool
pcl::WorldModel<PointT>::getExistingData(double previousOriginX, double previousOriginY, double previousOriginZ,
                           double offsetX, double offsetY, double offsetZ,
                           double volumeX, double volumeY, double volumeZ, pcl::PointCloud<PointT> &existing_slice)
{
  
  
  
  
  double newOriginX = previousOriginX + offsetX; double newOriginY = previousOriginY + offsetY; double newOriginZ = previousOriginZ + offsetZ;
  double newLimitX = newOriginX + volumeX - 1.0; double newLimitY = newOriginY + volumeY - 1.0; double newLimitZ = newOriginZ + volumeZ - 1.0;
  
  std::cout << std::endl << "==============Getting existing data for cube at [" << newOriginX <<  " " << newLimitX <<  "] [" << newOriginY <<  " " << newLimitY <<  "] [" << newOriginZ <<  " " << newLimitZ << "]" << std::endl;

  
  
  
  ///filter points in the space of the new cube
  PointCloudPtr newCube (new pcl::PointCloud<PointT>);
  // condition
  ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ))); 
  // build the filter
  pcl::ConditionalRemoval<PointT> condremAND (range_condAND, true);
  condremAND.setInputCloud (world);
  condremAND.setKeepOrganized(false);
  // apply filter
  condremAND.filter (*newCube);
  
  std::cout << "Cube contains " << newCube->points.size() << " points" << std::endl;
  
  ///filter points that belong to the new slice
  ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
  
  if(offsetX >= 0)
    range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE,  previousOriginX + volumeX - 1.0 )));
  else
    range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT,  previousOriginX )));
    
  if(offsetY >= 0)
    range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE,  previousOriginY + volumeY - 1.0 )));
  else
    range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT,  previousOriginY )));
    
  if(offsetZ >= 0)
    range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE,  previousOriginZ + volumeZ - 1.0 )));
  else
    range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT,  previousOriginZ )));
  
  // build the filter
  pcl::ConditionalRemoval<PointT> condrem (range_condOR, true);
  condrem.setInputCloud (newCube);
  condrem.setKeepOrganized(false);
  // apply filter
  condrem.filter (existing_slice);  
 
  if(existing_slice.points.size() != 0)
  {
    std::cout << "Saving already existing slice (" << existing_slice.points.size() << ") points." << std::endl;
    //~ pcl::io::savePCDFileASCII("slice_existing.pcd", *slice);
    //~ pcl::io::savePCDFile<pcl::PointXYZI> ("slice_existing.pcd", *slice, true);
    
    
    //transform the slice in new cube coordinates
    Eigen::Affine3f transformation; 
    transformation.translation()[0] = newOriginX;
    transformation.translation()[1] = newOriginY;
    transformation.translation()[2] = newOriginZ;
        
    transformation.linear() = Eigen::Matrix3f::Identity ();

    transformPointCloud(existing_slice, existing_slice, transformation.inverse());
    
     //~ pcl::io::savePCDFile<pcl::PointXYZI> ("slice_existing_transformed.pcd", *slice, true);
    
    return (true);
  }
  else
  {
    std::cout << "no existing data to return" << std::endl;
  }
 
  return (false);
 
}


template <typename PointT>
void 
pcl::WorldModel<PointT>::addCloud(double originX, double originY, double originZ, double sizeX, double sizeY, double sizeZ, PointCloudPtr new_cloud)
{
  std::cout << "Adding new cloud. Current world contains " << world->points.size() << " points" << std::endl;

  std::cout << "Adding new cloud in range: [" << originX << "-" << sizeX << "] ["  << originY << "-" << sizeY << "] ["  << originZ << "-" << sizeZ << "]" << std::endl;

  pcl::PointXYZI ptmin, ptmax;
  pcl::getMinMax3D	(	*new_cloud, ptmin, ptmax);
  std::cout << "slices's bounds: [" << ptmin.x << "-" << ptmax.x << "] ["  << ptmin.y << "-" << ptmax.y << "] [" << ptmin.z << "-" << ptmax.z << "]" << std::endl;
  std::cout << "new_slice contains: " << new_cloud->points.size() << " points" << std::endl;

  //~ cpt_updates++;
  //~ std::stringstream name2;
  //~ name2 << "slice_" << cpt_updates << ".pcd";
  //~ pcl::io::savePCDFile<pcl::PointXYZI> (name2.str(), *new_cloud, true);

  *world += *new_cloud;
/*
  if(world->points.size() != 0)
  {

    //set existing point in slice as nans
    setSliceAsNan(originX, originY, originZ, sizeX,  sizeY, sizeZ);
    
    //concatenate new points to the cloud
    *world += *new_cloud;
    
    std::cout << "world contains AFTER CONCATENATION : " << world->points.size() << " points" << std::endl;
    
    //~ cpt_updates++;
    //~ if(cpt_updates % 2 == 0)
    //~ {
      //~ std::cout << "-----------------------------------Removing nan points from the world (" << world->points.size() << " points)"<< std::endl;
      //~ std::vector<int> indices;
      //~ pcl::removeNaNFromPointCloud(	*world, *world, indices);		
      //~ std::cout << "Nan removal done. Cloud has " << world->points.size() << " points left" << std::endl;
    //~ }
    
  }
  else
  {
    
    *world = *new_cloud;
    
   
  }
  */
  std::cout << "World now contains " << world->points.size() << " points, of which a lot can be nans" << std::endl;
}

template <typename PointT>
void 
pcl::WorldModel<PointT>::setResolution(double resolution)
{
  octree.setResolution(resolution);
}

template <typename PointT>
double 
pcl::WorldModel<PointT>::getResolution()
{
  return (octree.getResolution());
}

template <typename PointT>
typename pcl::WorldModel<PointT>::PointCloudPtr 
pcl::WorldModel<PointT>::getWorld()
{
  return (world);
}




template <typename PointT>
void
pcl::WorldModel<PointT>::getWorldAsCubes(double size, std::vector<typename pcl::WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f> &transforms)
{

  std::cout << "World contains " << world->points.size() << " points (including nans)" << std::endl;

  //remove nans from world cloud
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud	(	*world, *world, indices);
	
   std::cout << "World contains " << world->points.size() << " points (without nans)" << std::endl;
  

  double cubeSide = size;
  if(cubeSide <= 0.0f)
  {
    std::cout << "Size must be positive and non null. Setting it to 3.0meters" << std::endl;
    cubeSide = 3.0f;
  }
  
  std::cout << "Cube size: " << cubeSide << std::endl;
  
  //get bounding values on XYZ
  PointT min, max;
  pcl::getMinMax3D(*world, min, max);

  std::cout <<"World contains " << world->points.size() << " points" << std::endl;
  std::cout <<"Bounding box for the world: " << std::endl
            << min.x << " - " << max.x << std::endl
            << min.y << " - " << max.y << std::endl
            << min.z << " - " << max.z << std::endl;


  PointT origin = min;
  
  // clear returned vectors
  cubes.clear();
  transforms.clear();

  // crop box filter used to extract each cube;
    //pcl::CropBox<PointT> filter;
    //filter.setInputCloud(world);
    //Eigen::Affine3f transform = Eigen::Affine3f::Identity(); // create transform
    //filter.setTransform(transform);
    //filter.setRotation(Eigen::Vector3f::Zero ());
    //filter.setTranslation(Eigen::Vector3f::Zero ());

  //iterate with box filter
  while(origin.x < max.x)
  {
    origin.y = min.y;
    while(origin.y < max.y)
    {
      origin.z = min.z;
      while(origin.z < max.z)
      {
        //extract cube here
        std::cout << "Extracting cube at: " << origin.x << " " << origin.y << " " << origin.z << std::endl;

        PointCloudPtr box (new pcl::PointCloud<PointT>);

          //filter.setMin(minBounds);
          //filter.setMax(maxBounds);

        // SET CONDITIONAL FILTER
        ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ());
        range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x)));
        range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide)));
        range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y)));
        range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide)));
        range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z)));
        range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide)));


        // build the filter
        pcl::ConditionalRemoval<PointT> condrem (range_cond);
        condrem.setInputCloud (world);
        condrem.setKeepOrganized(false);
        // apply filter
        condrem.filter (*box);

        if(box->points.size() > 0)
        {
           //filter.filter(*box);
          Eigen::Vector3f transform;
          transform[0] = origin.x; transform[1] = origin.y; transform[2] = origin.z;
          transforms.push_back(transform);
          cubes.push_back(box);        
        }
        else
        {
          std::cout << "Extracted cube was empty, skiping this one" << std::endl;
        }
        origin.z += cubeSide * 0.9;
        //std::cout << "new Z origin: " << origin.z << " max Z:" << max.z << std::endl;
      }
      //std::cout << "old Y origin: " << origin.y << " max Y:" << max.y << std::endl;
      origin.y += cubeSide * 0.9;
      //std::cout << "new Y origin: " << origin.y << " max Y:" << max.y << std::endl;
    }
    //std::cout << "old X origin: " << origin.x << " max X:" << max.x << std::endl;
    origin.x += cubeSide * 0.9;
    //std::cout << "new X origin: " << origin.x << " max X:" << max.x << std::endl;
  }


 /* for(int c = 0 ; c < cubes.size() ; ++c)
  {
    std::stringstream name;
    name << "cloud" << c+1 << ".pcd";
    pcl::io::savePCDFileASCII(name.str(), *(cubes[c]));
    
  }*/

  std::cout << "returning " << cubes.size() << " cubes" << std::endl;

}



#define PCL_INSTANTIATE_WorldModel(T) template class PCL_EXPORTS pcl::WorldModel<T>;

#endif // PCL_WORLD_MODEL_IMPL_HPP_
